/**
  ******************************************************************************
  * @file    gap_gate_driver_ctrl.c
  * @author  Motor Control SDK Team, ST Microelectronics
  * @brief   This file provides firmware functions that implement the following features
  *          of the GAP_GATE_DRIVER_CTRL component of the Motor Control SDK:
  *           < Add below the list of features, one per line.>
  *           +
  *           +
  *           +
  *           +
  *
  ******************************************************************************
  * @attention
  *
  * <h2><center>&copy; Copyright (c) 2019 STMicroelectronics.
  * All rights reserved.</center></h2>
  *
  * This software component is licensed by ST under Ultimate Liberty license
  * SLA0044, the "License"; You may not use this file except in compliance with
  * the License. You may obtain a copy of the License at:
  *                             www.st.com/SLA0044
  *
  ******************************************************************************
  */

/* Includes ------------------------------------------------------------------*/
#include "gap_gate_driver_ctrl.h"

#include "mc_type.h"

/** @addtogroup MCSDK
  * @{
  */

/** @defgroup GAP_GATE_DRIVER_CTRL <GAP_GATE_DRIVER_CTRL>
  * @brief GAP_GATE_DRIVER_CTRL component of the Motor Control SDK
  *
  * <Detailed documentation for the component. This detailed description documents the
  * component: it is its users guide. it explains the feratures it provides, how to use
  * it and what conditions are required fdor its usage.
  *
  * There shall be only one such section per component. If the implementation of a
  * component is split among several .c files, only one of them will have this section.
  * The others will use an @addtogroup GAP_GATE_DRIVER_CTRL instead.
  * >
  *
  * @{
  */

/* Private defines -----------------------------------------------------------*/
/**
  * @brief Brief description same rules as for structs
  *
  *  Detailed description if needed.
  *
  * As above: be direct, avoid the obvious, tell the hidden.
  */

#define GAP_STARTCONFIG                 0x2A
#define GAP_STOPCONFIG                  0x3A
#define GAP_WRITEREG                    0x80
#define GAP_READREG                     0xA0
#define GAP_RESETREG                    0xC0
#define GAP_RESETSTATUS                 0xD0
#define GAP_GLOBALRESET                 0xEA
#define GAP_SLPEEP                      0xF5
#define GAP_NOP                         0x00


#define GAP_ERROR_CODE_FROM_DEVICE_MASK (uint32_t)(0xFF000000)

#define WAITTIME                        5000 /* 860u */
#define WAITTRCHK                       50   /* 8.6u */
#define WAITTSENSECHK                   50   /* 8.6u */
#define WAITTGCHK                       50   /* 8.6u */
#define WAITTDESATCHK                   50   /* 8.6u */

/* Global variables ----------------------------------------------------------*/

static void GAP_SD_Deactivate( GAP_Handle_t * pHandle_t );
static void GAP_SD_Activate( GAP_Handle_t * pHandle_t );
static void GAP_CS_Deactivate( GAP_Handle_t * pHandle_t );
static void GAP_CS_Activate( GAP_Handle_t * pHandle_t );
static uint16_t GAP_SPI_Send( GAP_Handle_t * pHandle_t, uint16_t value );

/**
  * @brief  Check errors of GAP devices
  * @param  pHandle related object of class CGAP_GDC
  * @param  errorNow buffer of returned bitfields containing error flags
  *         coming from GAP device that are currently active.\n
  *         \link GAP_class_error_code Code errors\endlink.\n
  *         The buffer have to be provided from the caller.
  * @param  errorOccurred buffer of returned bitfields containing error flags
  *         coming from GAP device that are over.\n
  *         \link GAP_class_error_code Code errors\endlink.\n
  *         The buffer have to be provided from the caller.
  * @retval bool It returns false if an error occurs, otherwise return true.
  */
__weak bool GAP_CheckErrors( GAP_Handle_t * pHandle, uint32_t * error_now, uint32_t * error_occurred )
{
    bool ret_val = false;
    uint32_t errorFromDevices[MAX_DEVICES_NUMBER];

    if ( ( error_now ) && ( error_occurred ) )
    {
        uint8_t index1, index2;
        uint8_t ret_read[MAX_DEVICES_NUMBER];

//    /* If current error is device not programmable try to re-configure before
//       read the status registers */
//    if ((pDVars->wGAP_ErrorsNow[0] & GAP_ERROR_CODE_DEVICES_NOT_PROGRAMMABLE) ||
//        (pDVars->wGAP_ErrorsNow[0] & GAP_ERROR_CODE_SPI_CRC))
//    {
//      if (GAP_Configuration(pHandle))
//      {
//        pDVars->wGAP_ErrorsNow[0] &= ~GAP_ERROR_CODE_DEVICES_NOT_PROGRAMMABLE;
//        pDVars->wGAP_ErrorsNow[0] &= ~GAP_ERROR_CODE_SPI_CRC;
//      }
//    }


        index2 = pHandle->DeviceNum;
        ret_val = GAP_ReadRegs( pHandle, ret_read, STATUS1 );
        for ( index1 = 0; index1 < index2; index1 ++ )
        {
            errorFromDevices[index1] = ( ret_read[index1] << 16 );
        }
        ret_val = GAP_ReadRegs( pHandle, ret_read, STATUS2 );
        for ( index1 = 0; index1 < index2; index1 ++ )
        {
            /* Clear GATE bit from STATUS2 - no error if 1 */
            ret_read[index1] &= 0xFE;
            errorFromDevices[index1] |= ( ret_read[index1] << 8 );
        }
        ret_val = GAP_ReadRegs( pHandle, ret_read, STATUS3 );
        for ( index1 = 0; index1 < index2; index1 ++ )
        {
            errorFromDevices[index1] |= ret_read[index1];
        }

        for ( index1 = 0; index1 < index2; index1 ++ )
        {
            pHandle->GAP_ErrorsNow[index1] &= GAP_ERROR_CODE_FROM_DEVICE_MASK;
            pHandle->GAP_ErrorsNow[index1] |= errorFromDevices[index1];
            pHandle->GAP_ErrorsOccurred[index1] |= pHandle->GAP_ErrorsNow[index1];
            error_now[index1] = pHandle->GAP_ErrorsNow[index1];
            error_occurred[index1] = pHandle->GAP_ErrorsOccurred[index1];
        }
    }
    return ret_val;
}

/**
  * @brief  Clears the fault state of GAP devices.
  * @param  pHandle related object of class CGDC.
  * @retval none.
  */
__weak void GAP_FaultAck( GAP_Handle_t * pHandle )
{
    uint8_t index1, index2;
    uint16_t value;

    GAP_SD_Activate( pHandle );
    value = GAP_CRCCalculate( GAP_RESETSTATUS, 0xFF );
    GAP_CS_Activate( pHandle );
    index2 = pHandle->DeviceNum;

    for ( index1 = 0; index1 < index2; index1 ++ )
    {
        GAP_SPI_Send( pHandle, value );
    }
    GAP_CS_Deactivate( pHandle );
    GAP_SD_Deactivate( pHandle );
    wait( WAITTIME );

    for ( index1 = 0; index1 < index2; index1 ++ )
    {
        pHandle->GAP_ErrorsOccurred[index1] = GAP_ERROR_CLEAR;
    }
}

/**
  * @brief  Programs the GAP devices with the settled parameters.
  * @param  pHandle related object of class CGDC.
  * @retval bool It returns false if at least one device is not programmable,
  *         otherwise return true.
  */
__weak bool GAP_Configuration( GAP_Handle_t * pHandle )
{
    bool ret_val;

    LL_SPI_Enable(pHandle->SPIx);

    /* Configure devices with settled parameters */
    GAP_DevicesConfiguration( pHandle );

    /* Verify if device has been programmed */
    ret_val = GAP_IsDevicesProgrammed( pHandle );

    if ( !ret_val )
    {
        /* At least one device is not programmable */
        pHandle->GAP_ErrorsNow[0] |= GAP_ERROR_CODE_DEVICES_NOT_PROGRAMMABLE;
        pHandle->GAP_ErrorsOccurred[0] |= GAP_ERROR_CODE_DEVICES_NOT_PROGRAMMABLE;
    }
    return ret_val;
}

/**
  * @brief  Checks if the GAP devices are programmed with the settled parameters.
  * @param  pHandle related object of class CGDC.
  * @retval True if the GAP devices are already programmed with the settled
  *         parameters.
  */
__weak bool GAP_IsDevicesProgrammed( GAP_Handle_t * pHandle )
{
    bool ret_val = true;
    uint8_t index1, index2 = pHandle->DeviceNum;
    uint8_t read_reg_values[MAX_DEVICES_NUMBER];

    GAP_ReadRegs( pHandle, read_reg_values, CFG1 );

    for ( index1 = 0; index1 < index2; index1 ++ )
    {
        if ( ( pHandle->DeviceParams[index1].CFG1 & CFG1_REG_MASK ) != read_reg_values[index1] )
        {
            ret_val = false;
            break;
        }
    }

    if ( ret_val )
    {
        GAP_ReadRegs( pHandle, read_reg_values, CFG2 );
        for ( index1 = 0; index1 < index2; index1 ++ )
        {
            if ( ( pHandle->DeviceParams[index1].CFG2 & CFG2_REG_MASK ) != read_reg_values[index1] )
            {
                ret_val = false;
                break;
            }
        }
    }
    if ( ret_val )
    {
        GAP_ReadRegs( pHandle, read_reg_values, CFG3 );
        for ( index1 = 0; index1 < index2; index1 ++ )
        {
            if ( ( pHandle->DeviceParams[index1].CFG3 & CFG3_REG_MASK ) != read_reg_values[index1] )
            {
                ret_val = false;
                break;
            }
        }
    }
    if ( ret_val )
    {
        GAP_ReadRegs( pHandle, read_reg_values, CFG4 );
        for ( index1 = 0; index1 < index2; index1 ++ )
        {
            if ( ( pHandle->DeviceParams[index1].CFG4 & CFG4_REG_MASK ) != read_reg_values[index1] )
            {
                ret_val = false;
                break;
            }
        }
    }
    if ( ret_val )
    {
        GAP_ReadRegs( pHandle, read_reg_values, CFG5 );
        for ( index1 = 0; index1 < index2; index1 ++ )
        {
            if ( ( pHandle->DeviceParams[index1].CFG5 & CFG5_REG_MASK ) != read_reg_values[index1] )
            {
                ret_val = false;
                break;
            }
        }
    }
    if ( ret_val )
    {
        GAP_ReadRegs( pHandle, read_reg_values, DIAG1 );
        for ( index1 = 0; index1 < index2; index1 ++ )
        {
            if ( ( pHandle->DeviceParams[index1].DIAG1 & DIAG1_REG_MASK )  != read_reg_values[index1] )
            {
                ret_val = false;
                break;
            }
        }
    }
    if ( ret_val )
    {
        GAP_ReadRegs( pHandle, read_reg_values, DIAG2 );
        for ( index1 = 0; index1 < index2; index1 ++ )
        {
            if ( ( pHandle->DeviceParams[index1].DIAG2 & DIAG2_REG_MASK ) != read_reg_values[index1] )
            {
                ret_val = false;
                break;
            }
        }
    }
    return ret_val;
}

/**
  * @brief  Programs the GAP devices with the settled parameters.
  * @param  pHandle related object of class CGDC.
  * @retval bool It returns false if an error occurs, otherwise return true.
  */
__weak bool GAP_DevicesConfiguration( GAP_Handle_t * pHandle )
{
    bool ret_val = true;
    uint8_t index1, DeviceNum = pHandle->DeviceNum;
    uint8_t write_reg_values[MAX_DEVICES_NUMBER];

    GAP_StartConfig( pHandle );

    /* Global Reset before programming */
    GAP_GlobalReset( pHandle );

    for ( index1 = 0; index1 < DeviceNum; index1 ++ )
    {
        write_reg_values[index1] = pHandle->DeviceParams[index1].CFG1;
    }
    ret_val = GAP_WriteRegs( pHandle, write_reg_values, CFG1 );

    if ( ret_val )
    {
        for ( index1 = 0; index1 < DeviceNum; index1 ++ )
        {
            write_reg_values[index1] = pHandle->DeviceParams[index1].CFG2;
        }
        ret_val = GAP_WriteRegs( pHandle, write_reg_values, CFG2 );
    }

    if ( ret_val )
    {
        for ( index1 = 0; index1 < DeviceNum; index1 ++ )
        {
            write_reg_values[index1] = pHandle->DeviceParams[index1].CFG3;
        }
        ret_val = GAP_WriteRegs( pHandle, write_reg_values, CFG3 );
    }
    if ( ret_val )
    {
        for ( index1 = 0; index1 < DeviceNum; index1 ++ )
        {
            write_reg_values[index1] = pHandle->DeviceParams[index1].CFG4;
        }
        ret_val = GAP_WriteRegs( pHandle, write_reg_values, CFG4 );
    }
    if ( ret_val )
    {
        for ( index1 = 0; index1 < DeviceNum; index1 ++ )
        {
            write_reg_values[index1] = pHandle->DeviceParams[index1].CFG5;
        }
        ret_val = GAP_WriteRegs( pHandle, write_reg_values, CFG5 );
    }
    if ( ret_val )
    {
        for ( index1 = 0; index1 < DeviceNum; index1 ++ )
        {
            write_reg_values[index1] = pHandle->DeviceParams[index1].DIAG1;
        }
        ret_val = GAP_WriteRegs( pHandle, write_reg_values, DIAG1 );
    }
    if ( ret_val )
    {
        for ( index1 = 0; index1 < DeviceNum; index1 ++ )
        {
            write_reg_values[index1] = pHandle->DeviceParams[index1].DIAG2;
        }
        ret_val = GAP_WriteRegs( pHandle, write_reg_values, DIAG2 );
    }

    GAP_StopConfig( pHandle );

    /* Fault reset */
    GAP_FaultAck( pHandle );

    return ret_val;
}

/**
  * @brief  Calculate CRC from data and create 16bit value with data as MSB and
  *         CRC as LSB.
  * @param  data 8bit value used to calculate CRC.
  * @retval uint16_t It returns the 16bit value with data as MSB and
  *         CRC as LSB.
  */
__weak uint16_t GAP_CRCCalculate( uint8_t data, uint8_t crc_initial_value )
{
    uint8_t crc = crc_initial_value;
    uint8_t poly = 0x07;
    uint8_t crc_temp;
    uint8_t crctab[8];
    uint8_t index1, index2;

    uint16_t value;
    value = data;
    value <<= 8;

    for ( index2 = 0; index2 < 8; index2 ++ )
    {
        crctab[index2] = ( crc >> index2 ) & 0x1;
    }

    for ( index2 = 0; index2 < 8; index2 ++ )
    {
        crc_temp = ( crctab[7] << 7 ) + ( crctab[6] << 6 ) + ( crctab[5] << 5 ) + ( crctab[4] << 4 ) + ( crctab[3] << 3 ) +
                   ( crctab[2] << 2 ) + ( crctab[1] << 1 ) + ( crctab[0] );
        crctab[0] = ( ( data >> ( 7 - index2 ) ) & 0x1 ) ^ crctab[7];

        for ( index1 = 1; index1 < 8; index1 ++ )
        {
            crctab[index1] = ( crctab[0] & ( ( poly >> index1 ) & 0x1 ) ) ^ ( ( crc_temp >> ( index1 - 1 ) ) & 0x1 );
        }
    }

    crc = ( crctab[7] << 7 ) + ( crctab[6] << 6 ) + ( crctab[5] << 5 ) + ( crctab[4] << 4 ) + ( crctab[3] << 3 ) +
          ( crctab[2] << 2 ) + ( crctab[1] << 1 ) + ( crctab[0] << 0 );
    crc ^= 0xFF;

    value |= crc;
    return value;
}

/**
  * @brief  Verify the CRC from dataIn and extract the 8bit data value (out).
  * @param  out Reference for the extracted 8bit data value.
  * @param  dataIn 16bit value with data as MSB and CRC as LSB.
  * @retval bool It returns true if CRC is correct, false otherwise.
  */
__weak bool GAP_CRCCheck( uint8_t * out, uint16_t data_in )
{
    bool ret_val = false;
    uint8_t data = ( uint8_t )( data_in >> 8 );
    uint8_t received_crc = ( uint8_t )( data_in );
    uint8_t crc = ( uint8_t )( GAP_CRCCalculate( data, 0xFF ) ) ^ 0xFF;

    if ( crc == received_crc )
    {
        *out = data;
        ret_val = true;
    }
    return ret_val;
}

/**
  * @brief  Wait a time interval proportional to count.
  * @param  count Number of count to be waited
  * @retval none
  */
__weak void wait( uint16_t count )
{
    volatile uint16_t wait_cnt;

    for ( wait_cnt = 0; wait_cnt < count; wait_cnt ++ )
    {
    }
}

/**
  * @brief  Return the register mask starting from it address.
  * @param  reg Register address.
  * @retval uint8_t Mask to be and-ed bit wise to data to filter it.
  */
__weak uint8_t GAP_RegMask( GAP_Registers_Handle_t reg )
{
    uint8_t ret_val;
    switch ( reg )
    {
    case CFG1:
    {
        ret_val = CFG1_REG_MASK;
    }
    break;
    case CFG2:
    {
        ret_val = CFG2_REG_MASK;
    }
    break;
    case CFG3:
    {
        ret_val = CFG3_REG_MASK;
    }
    break;
    case CFG4:
    {
        ret_val = CFG4_REG_MASK;
    }
    break;
    case CFG5:
    {
        ret_val = CFG5_REG_MASK;
    }
    break;
    case STATUS1:
    {
        ret_val = STATUS1_REG_MASK;
    }
    break;
    case STATUS2:
    {
        ret_val = STATUS2_REG_MASK;
    }
    break;
    case STATUS3:
    {
        ret_val = STATUS3_REG_MASK;
    }
    break;
    case TEST1:
    {
        ret_val = TEST1_REG_MASK;
    }
    break;
    case DIAG1:
    {
        ret_val = DIAG1_REG_MASK;
    }
    break;
    case DIAG2:
    {
        ret_val = DIAG2_REG_MASK;
    }
    break;
    default:
    {
        ret_val = 0x00;
    }
    break;
    }
    return ret_val;
}

/**
  * @brief  Read all data in the daisy chain related to register reg.
  * @param  pHandle related object of class CGAP_GDC
  * @param  pDataRead Pointer to the buffer in which will be stored the readed
  *         data. The buffer have to be provided from the caller.
  * @param  reg Register to be read. It must be one of the register defined in
  *         \link GAP_class_private_enum GAP_Registers_t\endlink.
  * @retval bool It returns false if an error occurs, otherwise return true.
  */
__weak bool GAP_ReadRegs( GAP_Handle_t * pHandle, uint8_t * pDataRead, GAP_Registers_Handle_t reg )
{
    bool ret_val = false;
    uint8_t index1;
    uint8_t device;
    uint8_t data;
    uint16_t value;

    if ( pDataRead )
    {
        value = GAP_CRCCalculate( GAP_READREG | reg, 0xFF );
        GAP_CS_Activate( pHandle );

        for ( index1 = 0; index1 < pHandle->DeviceNum; index1 ++ )
        {
            GAP_SPI_Send( pHandle, value );
        }

        GAP_CS_Deactivate( pHandle );
        wait( WAITTIME );
        value = GAP_CRCCalculate( GAP_NOP, 0xFF );
        GAP_CS_Activate( pHandle );

        ret_val = true;

        for ( index1 = 0; index1 < pHandle->DeviceNum; index1 ++ )
        {
            device = pHandle->DeviceNum - index1 - 1;

            if ( pHandle->DeviceParams[device].CFG1 & GAP_CFG1_CRC_SPI )
            {
                if ( GAP_CRCCheck( &data, GAP_SPI_Send( pHandle, value ) ) )
                {
                    pDataRead[device] = data & GAP_RegMask( reg );
                }
                else
                {
                    pDataRead[device] = 0x00;
                    pHandle->GAP_ErrorsNow[0] |= GAP_ERROR_CODE_SPI_CRC;
                    pHandle->GAP_ErrorsOccurred[0] |= GAP_ERROR_CODE_SPI_CRC;
                    ret_val = false;
                }
            }
            else
            {
                pDataRead[device] = ( uint8_t )( GAP_SPI_Send( pHandle, value ) >> 8 ) & GAP_RegMask( reg );
            }
        }
        GAP_CS_Deactivate( pHandle );
    }
    return ret_val;
}

__weak void GAP_StartConfig( GAP_Handle_t * pHandle )
{
    uint8_t index;
    uint16_t value;

    GAP_SD_Activate( pHandle );

    value = GAP_CRCCalculate( GAP_STARTCONFIG, 0xFF );
    GAP_CS_Activate( pHandle );

    for ( index = 0; index < pHandle->DeviceNum; index ++ )
    {
        GAP_SPI_Send( pHandle, value );
    }
    GAP_CS_Deactivate( pHandle );
    wait( WAITTIME );
}

__weak void GAP_StopConfig( GAP_Handle_t * pHandle )
{
    uint8_t index;
    uint16_t value;

    value = GAP_CRCCalculate( GAP_STOPCONFIG, 0xFF );
    GAP_CS_Activate( pHandle );

    for ( index = 0; index < pHandle->DeviceNum; index ++ )
    {
        GAP_SPI_Send( pHandle, value );
    }
    GAP_CS_Deactivate( pHandle );
    wait( WAITTIME );
    GAP_SD_Deactivate( pHandle );
}

/**
  * @brief  Write data in the daisy chain in to the register reg.
  * @param  pHandle related object of class CGAP_GDC
  * @param  pDataWrite Pointer to the buffer in which are stored the data
  *         to be written in the register reg. The buffer have to be provided
  *         from the caller.
  * @param  reg Register to be write. It must be one of the register defined in
  *         \link GAP_class_private_enum GAP_Registers_Handle_t\endlink.
  * @retval bool It returns false if an error occurs, otherwise return true.
  */
__weak bool GAP_WriteRegs( GAP_Handle_t * pHandle, uint8_t * pDataWrite, GAP_Registers_Handle_t reg )
{
    bool ret_val = false;

    if ( pDataWrite )
    {
        uint8_t crc;
        uint8_t index;
        uint16_t value;

        value = GAP_CRCCalculate( GAP_WRITEREG | reg, 0xFF );
        crc = ( uint8_t )( value );
        GAP_CS_Activate( pHandle );

        for ( index = 0; index < pHandle->DeviceNum; index ++ )
        {
            GAP_SPI_Send( pHandle, value );
        }
        GAP_CS_Deactivate( pHandle );
        wait( WAITTIME );

        GAP_CS_Activate( pHandle );
        ret_val = true;

        for ( index = 0; index < pHandle->DeviceNum; index ++ )
        {
            value = GAP_CRCCalculate( pDataWrite[index], crc ^ 0xFF );
            GAP_SPI_Send( pHandle, value );
        }

        GAP_CS_Deactivate( pHandle );
        wait( WAITTIME );
    }
    return ret_val;
}

/**
  * @brief  Reset all the registers to the default and releases all the failure flag.
  * @param  pHandle related object of class CGDC.
  * @retval none.
  */
__weak void GAP_GlobalReset( GAP_Handle_t * pHandle )
{
    uint8_t index;
    uint16_t value;

    value = GAP_CRCCalculate( GAP_GLOBALRESET, 0xFF );
    GAP_CS_Activate( pHandle );

    for ( index = 0; index < pHandle->DeviceNum; index ++ )
    {
        GAP_SPI_Send( pHandle, value );
    }
    GAP_CS_Deactivate( pHandle );
    wait( WAITTIME );
}

/**
  * @brief  Reset selected status register.
  * @param  pHandle related object of class CGAP_GDC
  * @param  reg Register to be reset. It must be one of the STATUS register
   *        defined in \link GAP_class_private_enum GAP_Registers_Handle_t\endlink.
  * @retval bool It returns false if an error occurs, otherwise return true.
  */
__weak bool GAP_ResetStatus( GAP_Handle_t * pHandle, GAP_Registers_Handle_t reg )
{
    bool ret_val = false;
    uint8_t index;
    uint16_t value;
    GAP_SD_Activate( pHandle );
    value = GAP_CRCCalculate( GAP_RESETREG | reg, 0xFF );
    GAP_CS_Activate( pHandle );
    for ( index = 0; index < pHandle->DeviceNum; index ++ )
    {
        GAP_SPI_Send( pHandle, value );
    }
    GAP_CS_Deactivate( pHandle );
    GAP_SD_Deactivate( pHandle );
    return ret_val;
}

/**
  * @brief  ** pHandle function should be called just in IDLE state. **\n
  *         It performs the selected test on each GAP devices.
  * @param  pHandle related object of class CGDC
  * @param  testMode Test mode to be executed. It shall be one of the
  *         test modes present in the
  *         \link GAP_class_testModes GAP class test modes\endlink.
  * @retval bool It returns true if an error occurs, otherwise return false.
  */
__weak bool GAP_Test( GAP_Handle_t * pHandle, GAP_TestMode_t testMode )
{
    bool ret_val = false;
    bool invertResult = false;

    uint8_t testModeData;
    uint8_t clr[MAX_DEVICES_NUMBER];
    uint8_t data[MAX_DEVICES_NUMBER];
    uint8_t index1, index2 = pHandle->DeviceNum;
    uint8_t statusMask;
    uint16_t wait_cnt;

    switch ( testMode )
    {
    case GOFF_CHK:
    {
        testModeData = GAP_TEST1_GOFFCHK;
        wait_cnt = WAITTGCHK;
        statusMask = GAP_STATUS1_DESAT;
    }
    break;
    case GON_CHK:
    {
        testModeData = GAP_TEST1_GONCHK;
        wait_cnt = WAITTGCHK;
        statusMask = GAP_STATUS1_TSD;
    }
    break;
    case GAP_TEST1_DESCHK:
    {
        testModeData = DESAT_CHK;
        wait_cnt = WAITTDESATCHK;
        statusMask = GAP_STATUS1_DESAT;
        invertResult = true;
    }
    break;
    case SENSE_RESISTOR_CHK:
    {
        testModeData = GAP_TEST1_RCHK;
        wait_cnt = WAITTRCHK;
        statusMask = GAP_STATUS1_SENSE;
    }
    break;
    case SENSE_COMPARATOR_CHK:
    {
        testModeData = GAP_TEST1_SNSCHK;
        wait_cnt = WAITTSENSECHK;
        statusMask = GAP_STATUS1_SENSE;
        invertResult = true;
    }
    break;
    default:
    {
        ret_val = true;
    }
    break;
    }

    for ( index1 = 0; index1 < index2; index1 ++ )
    {
        data[index1] = testModeData;
        clr[index1] = 0x00;
    }
    GAP_WriteRegs( pHandle, data, TEST1 );
    wait( wait_cnt );
    GAP_ReadRegs( pHandle, data, STATUS1 );
    /* Clear TEST1 regs */
    GAP_WriteRegs( pHandle, clr, TEST1 );
    /* Clear STATUS1 regs */
    GAP_ResetStatus( pHandle, STATUS1 );

    for ( index1 = 0; index1 < index2; index1 ++ )
    {
        if ( invertResult )
        {
            if ( ( data[index1] & statusMask ) == 0 )
            {
                ret_val = true;
            }
        }
        else
        {
            if ( ( data[index1] & statusMask ) != 0 )
            {
                ret_val = true;
            }
        }
    }

    return ret_val;
}

/**
* @brief  This function sends a 16bit value through the configured SPI and
*         returns the 16-bit value received during communication.
* @param  pHandle_t: related object of class CGDC
* @param  value: Value to be sent through SPI
* @retval uint16_t Received 16bit value
*/
uint16_t GAP_SPI_Send( GAP_Handle_t * pHandle_t, uint16_t value )
{
    SPI_TypeDef * SPIx = pHandle_t->SPIx;
    /* Wait for SPI Tx buffer empty */
    while (  LL_SPI_IsActiveFlag_TXE( SPIx ) == 0 );
    /* Send SPI data */
    LL_SPI_TransmitData16 (SPIx, value );
    /* Wait for SPIz data reception */
    while ( LL_SPI_IsActiveFlag_RXNE( SPIx ) == 0 );
    /* Read SPIz received data */
    return  LL_SPI_ReceiveData16( SPIx );
}

/**
* @brief  Deactivate SD pin
* @param  pHandle_t: related object of class CGDC
* @retval none
*/
void GAP_SD_Deactivate( GAP_Handle_t * pHandle_t )
{
    LL_GPIO_SetOutputPin( pHandle_t->NSDPort, pHandle_t->NSDPin );
}

/**
* @brief  Activate SD pin
* @param  pHandle_t: related object of class CGDC
* @retval none
*/
void GAP_SD_Activate( GAP_Handle_t * pHandle_t )
{
    LL_GPIO_ResetOutputPin( pHandle_t->NSDPort, pHandle_t->NSDPin );
}

/**
* @brief  Deactivate CS pin
* @param  pHandle_t: related object of class CGDC
* @retval none
*/
void GAP_CS_Deactivate( GAP_Handle_t * pHandle_t )
{
    LL_GPIO_SetOutputPin( pHandle_t->NCSPort, pHandle_t->NCSPin );
}

/**
* @brief  Activate CS pin
* @param  pHandle_t: related object of class CGDC
* @retval none
*/
void GAP_CS_Activate( GAP_Handle_t * pHandle_t )
{
    LL_GPIO_ResetOutputPin( pHandle_t->NCSPort, pHandle_t->NCSPin );
}

/**
  * @}
  */

/**
  * @}
  */

/************************ (C) COPYRIGHT 2019 STMicroelectronics *****END OF FILE****/

